package model;

/**
 * @author wujiazhan
 * @className InvertedPendulum
 * @description 倒立摆模型
 * @date 2021-05-29 11:13
 */
@SuppressWarnings("all")
public class InvertedPendulum {

    private ModelParameter self;
    private Integrator theta_Integrator;
    private Integrator theta_1_Integrator;
    private Integrator x_Integrator;
    private Integrator x_1_Integrator;

    public InvertedPendulum(ModelParameter modelParameter){
        this.self = modelParameter;
        this.theta_Integrator = new Integrator(self.samplingTime);
        this.theta_1_Integrator = new Integrator(self.samplingTime);
        this.x_Integrator = new Integrator(self.samplingTime);
        this.x_1_Integrator = new Integrator(self.samplingTime);
    }

    public void forwardAndIntegral(double F){
        double theta_2 = theta_2_val(self.theta, self.theta_1, F);//迭代摆角（得出摆角二阶微分）
        double x_2 = x_2_val(self.theta, self.theta_1, F);//迭代位置（得出位置二阶微分）
        self.theta_1 = theta_1_Integrator.getIntegralSum(theta_2);//积分得到一阶微分
        self.x_1 = x_1_Integrator.getIntegralSum(x_2);//积分得到一阶微分
        self.theta = theta_Integrator.getIntegralSum(self.theta_1);
        self.x_car = x_Integrator.getIntegralSum(self.x_1);
    }



    public void forwardAndRKIntegral(double F){
        double rk_h = self.samplingTime;

        double theta_k1 = self.theta_1;
        double x_k1 = self.x_1;

        double theta_l1 = theta_2_val(self.theta, self.theta_1, F);
        double x_l1 = x_2_val(self.theta, self.theta_1, F);

        double theta_k2 = self.theta_1 + theta_l1 * rk_h / 2;
        double x_k2 = self.x_1 + x_l1 * rk_h / 2;

        double theta_l2 = theta_2_val(self.theta + theta_k1 * rk_h / 2, theta_k2, F);
        double x_l2 = x_2_val(self.theta + theta_k1 * rk_h / 2, theta_k2, F);

        double theta_k3 = self.theta_1 + theta_l2 * rk_h / 2;
        double x_k3 = self.x_1 + x_l2 * rk_h / 2;

        double theta_l3 = theta_2_val(self.theta + theta_k2 * rk_h / 2, theta_k3, F);
        double x_l3 = x_2_val(self.theta + theta_k2 * rk_h / 2, theta_k3, F);

        double theta_k4 = self.theta_1 + theta_l3 * rk_h;
        double x_k4 = self.x_1 + x_l3 * rk_h;

        double theta_l4 = theta_2_val(self.theta + theta_k3 * rk_h, theta_k4, F);
        double x_l4 = x_2_val(self.theta + theta_k3 * rk_h, theta_k4, F);

        self.theta += (theta_k1 + 2 * theta_k2 + 2 * theta_k3 + theta_k4) * rk_h / 6;
        self.theta_1 += (theta_l1 + 2 * theta_l2 + 2 * theta_l3 + theta_l4) * rk_h / 6;

        self.x_car += (x_k1 + 2*x_k2 + 2*x_k3 + x_k4) * rk_h / 6;
        self.x_1 += (x_l1 + 2*x_l2 + 2*x_l3 + x_l4) * rk_h / 6;
    }

    /**
     * theta二阶微分
     * @param theta 摆角θ
     * @param theta_1 摆角θ一阶微分
     * @param F 力
     * @return theta二阶微分
     */
    private double x_2_val(double theta, double theta_1, double F){
        double sin_theta = Math.sin(theta);
        double cos_theta = Math.cos(theta);

        double num = self.J_ml2 * F + self.l_stick * self.M_stick * self.J_ml2 * sin_theta * Math.pow(theta_1,2)
                - self.m2l2 * self.G * sin_theta * cos_theta;

        double den = self.J_ml2 * (self.M_car + self.M_stick) - self.m2l2 * Math.pow(cos_theta,2);

        return num / den;
    }

    /**
     * 位置x二阶微分
     * @param theta 摆角θ
     * @param theta_1 摆角θ一阶微分
     * @param F 力
     * @return 位置x二阶微分
     */
    private double theta_2_val(double theta, double theta_1, double F){
        double sin_theta = Math.sin(theta);
        double cos_theta = Math.cos(theta);

        double num = self.M_stick * self.l_stick * cos_theta * F + self.m2l2 * sin_theta * cos_theta * Math.pow(theta_1,2)
                - (self.M_car + self.M_stick) * self.mlg * sin_theta;

        double den = self.m2l2 * Math.pow(cos_theta,2) - (self.M_car + self.M_stick) * self.J_ml2;

        return num / den;
    }

    //获取小车位置
    public double getX_car(){
        return self.x_car;
    }
    //获取摆角
    public double getTheta(){
        return self.theta;
    }
    //获取车速度,但是这个只能从外部获取
//    public double getCarVelocity(){
//        return self.x_1;
//    }
//    获取位置一阶微分,但是这个只能从外部获取
//    public double getX_1(){
//        return self.x_1;
//    }
//    获取摆角一阶微分,但是这个只能从外部获取
//    public double getTheta_1(){
//        return self.theta_1;
//    }
}
